#pragma once
#include <iostream>
#include "epigraph.hpp"
#include "discrete_linear_system.h"
#include "continuous_linear_system.h"
//using namespace cvx;
namespace auto_ros
{
namespace control
{
class LinearSteadyState
{
public:
	LinearSteadyState(std::shared_ptr<ContinuousLinearSys> linear_sys_ptr);
	bool add_slack_states(std::vector<int> state_index_vec);
	bool set_distur_vec(Eigen::VectorXd distur_vec);
	bool calc_steady_state(const Eigen::VectorXd ditur_vec,
						   Eigen::VectorXd &steady_state,
						   Eigen::VectorXd &steady_ctr,
						   Eigen::VectorXd &slack_vec);
	bool init_solver_ptr();
	cvx::OptimizationProblem qp_;
	cvx::VectorX x_;
	cvx::VectorX u_;

private:
	std::shared_ptr<ContinuousLinearSys> linear_sys_ptr_;
	cvx::VectorX s_;
	Eigen::VectorXd distur_vec_;
	Eigen::VectorXd out_vec_;
	std::shared_ptr<cvx::osqp::OSQPSolver> solver_ptr_;
};
} // namespace control
} // namespace auto_ros